package de.fh.ka.as.car.sensor;

import java.util.ArrayList;
import java.util.Date;
import java.util.List;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

/**
 * Pretty bad Speedometer Sensor
 * 
 * Needs a few seconds in a stable position to initialize. After that the mobile phone can be moved, but
 * shouldn't be rotated in z-axis. The speed gets calculated, but the andriod sensors noise is screwing with
 * the right speed pretty fast. If a right speed is available, you can set it with setCurrentSpeed. If you are
 * driving with a constant speed, you can pause it so that the noise doesn't change the speed
 */
public class Speedometer implements AndroidSensor {

	private SensorManager sensorManager = null;
	private List<Sensor> sensors = new ArrayList<Sensor>();
	private Date lastUpdate = new Date(System.currentTimeMillis());

	/**
	 * Acceleration from sensors
	 */
	private float[] mAcceleration;
	/**
	 * Acceleration from sensors - gravity
	 */
	private float[] mLinearAcceleration = new float[3];
	/**
	 * Speed in the directions value in m/s
	 */
	private double[] velocity = { 0.0, 0.0, 0.0 };
	double speed = 0;

	/** Called when the activity is first created. */
	public Speedometer(SensorManager sensorManager) {
		this.sensorManager = sensorManager;

		sensors.add(sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER));
		sensors.add(sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD));
	}

	public void enableSensors() {
		for (Sensor sensor : sensors)
			sensorManager.registerListener(speedListener, sensor, SensorManager.SENSOR_DELAY_NORMAL);
		lastUpdate = new Date(System.currentTimeMillis());
	}

	public void disableSensors() {
		sensorManager.unregisterListener(speedListener);
	}

	public double getCurrentSpeed() {
		return speed;
	}

	public void setCurrentSpeed(double speed) {
		// Full speed in x direction
		velocity[0] = speed;
		velocity[1] = 0;
		velocity[2] = 0;
	}

	@Override
	public double[] getSensorData() {
		double[] data = new double[1];
		data[0] = speed;
		return data;
	}

	@Override
	public SensorType getType() {
		return SensorType.Speedometer;
	}

	private final SensorEventListener speedListener = new SensorEventListener() {

		private double[] appliedAcceleration = { 0.0, 0.0, 0.0 };
		private double[] currentAcceleration = { 0.0, 0.0, 0.0 };

		// Variables for the calculation of constant gravity and sensor error.
		// This is then subtracted from every sensor data
		private final int gravityInitCylces = 60;
		private int gravityInitCylce = 0;
		private float[] mGravity = null;
		private float[][] mGravityCalculation = new float[gravityInitCylces][];

		@Override
		public void onAccuracyChanged(Sensor sensor, int accuracy) {
		}

		@Override
		public void onSensorChanged(SensorEvent event) {
			mAcceleration = event.values;

			// test with filter
			// getSpeedWithHighPass(event);
			// if (true)
			// return;

			if (mGravity == null) {
				// gravity constant is getting calculated, first remember a few cylce
				// and then get the average
				if (gravityInitCylce < gravityInitCylces) {
					mGravityCalculation[gravityInitCylce] = event.values;
					gravityInitCylce++;
				} else {
					// calc sum of all
					mGravity = new float[3];
					for (float[] value : mGravityCalculation) {
						mGravity[0] += value[0];
						mGravity[1] += value[1];
						mGravity[2] += value[2];
					}
					// get average
					mGravity[0] /= gravityInitCylces;
					mGravity[1] /= gravityInitCylces;
					mGravity[2] /= gravityInitCylces;
					// set time
					lastUpdate = new Date(System.currentTimeMillis());
				}
			} else {
				// extract gravity
				for (int i = 0; i < mLinearAcceleration.length; i++)
					mLinearAcceleration[i] = mAcceleration[i] - mGravity[i];

				// get delta time
				Date timeNow = new Date(System.currentTimeMillis());
				long timeDelta = timeNow.getTime() - lastUpdate.getTime();
				lastUpdate.setTime(timeNow.getTime());

				// calc speed gain via the time and current acceleration
				for (int i = 0; i < mLinearAcceleration.length; i++) {
					appliedAcceleration[i] = mLinearAcceleration[i] * ((double) timeDelta / 1000);
				}

				// add to speed
				for (int i = 0; i < appliedAcceleration.length; i++) {
					velocity[i] += appliedAcceleration[i];
				}

				// calc in km/h
				speed = Math.sqrt(Math.pow(velocity[0], 2) + Math.pow(velocity[1], 2));
				speed = speed * 3600 / 1000;
			}
		}

		// ramp-speed - play with this value until satisfied
		final float kFilteringFactor = 0.1f;

		// last result storage - keep definition outside of this function, eg. in wrapping object
		float[] accel = { 0.0f, 0.0f, 0.0f };
		float[] result = { 0.0f, 0.0f, 0.0f };

		/**
		 * Test with Filter
		 * 
		 * @param event
		 */
		private void getSpeedWithHighPass(SensorEvent event) {

			// acceleration.x,.y,.z is the input from the sensor
			// result.x,.y,.z is the filtered result

			// high-pass filter to eleminate gravity
			accel[0] = event.values[0] * kFilteringFactor + accel[0] * (1.0f - kFilteringFactor);
			accel[1] = event.values[1] * kFilteringFactor + accel[1] * (1.0f - kFilteringFactor);
			accel[2] = event.values[2] * kFilteringFactor + accel[2] * (1.0f - kFilteringFactor);
			result[0] = event.values[0] - accel[0];
			result[1] = event.values[1] - accel[1];
			result[2] = event.values[2] - accel[2];

			Date timeNow = new Date(System.currentTimeMillis());
			long timeDelta = timeNow.getTime() - lastUpdate.getTime();
			lastUpdate.setTime(timeNow.getTime());

			for (int i = 0; i < result.length; i++) {
				appliedAcceleration[i] = result[i] * ((double) timeDelta / 1000);
			}
			// double velocities[] = new double[mLinearAcceleration.length];
			for (int i = 0; i < appliedAcceleration.length; i++) {
				velocity[i] += appliedAcceleration[i];
			}

			double speed = Math.sqrt(Math.pow(velocity[0], 2) + Math.pow(velocity[1], 2));
			speed = speed * 3600 / 1000;
		}
	};

}